/*
 * This code is for a 4-wheel drive car as a line follower
 * Using a tracking line sensor, a 2-channel DC motor drive L2911
 * a HC-SR04 ultrasonic senser and 2 130 Micromotors, one is for moving 
 * foreward and backward, another is sterring
 * 
 * Connection:
 * Arduino NANO as a receiver 21.03.2025
 *    Motor Drive L9110 A-1A for 130 Micromotor        - D3
 *    Motor Drive L9110 A-1B for 130 Micromotor        - D6
 *    Motor Drive L9110 B-1A for 130 Micromotor        - D5
 *    Motor Drive L9110 B-1B for 130 Micromotor        - D7
 *
 * Ultrasonic sensor HC-SR04
 *    VCC
 *    Trig  D4
 *    Echo  D8
 *    GND
 * 
 * Tracking line sensor
 * TrackSensorLeftPin1  =  A2;  //定义左边第一个循迹红外传感器引脚X4为A2
 * TrackSensorLeftPin2  =  A1;  //定义左边第二个循迹红外传感器引脚X3为A1
 * TrackSensorRightPin1 =  A3;  //定义右边第一个循迹红外传感器引脚X1为A3 
 * TrackSensorRightPin2 =  A4;  //定义右边第二个循迹红外传感器引脚X2为A4
 *
 * History:
 * V0.0 KendinYap https://www.rcpano.net/2020/02/17/simple-and-cheap-radio-control-making-for-rc-models-diy-rc/
 * V0.1 25.06.2023 SH first adaption
 * V1.0 21.03.2025 SH Adapter to line follower car
 */


// ４ Channel Reciever

#include <SPI.h>
#include "printf.h"
#include "RF24.h"
#include <Servo.h>

// Add xxx.h into the same folder of ino file
// then include "xxx.h"
//#include "nRF24L01.h"
//#include "RF24.h"

//Line follower pin out 循迹红外引脚定义
//TrackSensorLeftPin1 TrackSensorLeftPin2 TrackSensorRightPin1 TrackSensorRightPin2
//      A2                  A1                  A3                   A4
const int TrackSensorLeftPin1  =  A2;  //定义左边第一个循迹红外传感器引脚X4为A2
const int TrackSensorLeftPin2  =  A1;  //定义左边第二个循迹红外传感器引脚X3为A1
const int TrackSensorRightPin1 =  A3;  //定义右边第一个循迹红外传感器引脚X1为A3 
const int TrackSensorRightPin2 =  A4;  //定义右边第二个循迹红外传感器引脚X2为A4
//定义各个循迹红外引脚采集的数据的变量
int TrackSensorLeftValue1;
int TrackSensorLeftValue2;
int TrackSensorRightValue1;
int TrackSensorRightValue2;

//Motor Drive L9110 pinout and Using Motor A port to 130 micromotor
#define D3 3 // To Motor Drive L9110 A-1A for 130 Micromotor
#define D6 6 // To Motor Drive L9110 A-1B for 130 Micromotor
#define D5 5 // To Motor Drive L9110 B-1A for 130 Micromotor
#define D7 7 // To Motor Drive L9110 B-1B for 130 Micromotor
// Ultrasonic sensor HC-SR04
const int trigPin = 4;  
const int echoPin = 8; 
unsigned long Time_Echo_us = 0;
unsigned long Length_mm_X100 = 0;
unsigned long Length_Integer = 0;
unsigned int Length_Fraction = 0;
float duration, distance;  //duration and distance, which will hold the length of the sound wave and how far away the object is. 
// nRF definition
//IMPORTANT: The same as in the transmitter 0xE9E8F0F0E1LL
const uint64_t pipeIn = 0xE9E8F0F0E1LL;
// Let these addresses be used for the pair
 uint8_t address[][6] = { "1Node", "2Node" };
// It is very helpful to think of an address as a path instead of as
// an identifying device destination
// Nano: CE   - 7  CSN  - 8
// Mega: CE   - 7  CSN  - 8   
// Nano shield: CE   - D9 CSN  - D10  
#define CE_PIN 9 //53
#define CSN_PIN 10 //49
// instantiate an object for the nRF24L01 transceiver
RF24 radio(CE_PIN, CSN_PIN);

// For testing communication, we'll be using a payload containing
// a single float number that will be incremented
// on every successful transmission
float payload = 0.0;

int ch_width_1 = 0, ch_width_2 = 0, ch_width_3 = 0, ch_width_4 = 0, ch_width_5 = 0, ch_width_6 = 0;

Servo ch1; Servo ch2; Servo ch3; Servo ch4; Servo ch5; Servo ch6;

// 6 channel
struct Signal {
  byte throttle;
  byte pitch;
  byte roll;
  byte yaw;
  byte gyr; 
  byte pit;
  byte MovingSpeed_A1A;
  byte MovingSpeed_A1B;
  byte Turn_B1A;
  byte Turn_B1B;

};

Signal data;

// Define the initial value of each data input.
void ResetData()
{
  // The middle position for Potential meters. (254/2=127)
  data.throttle = 0; // Motor Stop when Signal lost | 信号丢失时，关闭油门Throttle)
  data.pitch = 0; // Center (Signal lost position =127 | 俯仰通道Elevator)
  data.roll = 0; // Center (Signal lost position | 横滚通道Aileron(中心点127))
  data.yaw = 0; // Center (Signal lost position | 航向通道Rudder )
  data.gyr = 0; // Channel 5
  data.pit = 0; //Channel 6
  data.MovingSpeed_A1A = 0;
  data.MovingSpeed_A1B = 0;
  data.Turn_B1A = 0;
  data.Turn_B1B = 0;
} // ResetData


void setup(){
  
  Serial.begin(9600); //115200
  while (!Serial) {
    // some boards need to wait to ensure access to serial over USB
  }

  // Set the pins for each PWM signal
  //ch1.attach(2); //D2: THR
  //ch2.attach(3); //D3: RUD = Roll/Rudder/Tail
  //ch3.attach(4); //D4: ELE = Pitch/Elevator
  //ch4.attach(5); //D5: AIL = Yaw/Aileron
  pinMode(D3, OUTPUT);
  pinMode(D6, OUTPUT);
  pinMode(D5, OUTPUT);
  pinMode(D7, OUTPUT);
  //ch5.attach(6);
  //ch6.attach(9);

  //定义四路循迹红外传感器为输入接口
  pinMode(TrackSensorLeftPin1, INPUT);
  pinMode(TrackSensorLeftPin2, INPUT);
  pinMode(TrackSensorRightPin1, INPUT);
  pinMode(TrackSensorRightPin2, INPUT);

  //四路循迹红外传感器初始化为高电平
  digitalWrite(TrackSensorLeftPin1, HIGH);
  digitalWrite(TrackSensorLeftPin2, HIGH);
  digitalWrite(TrackSensorRightPin1, HIGH);
  digitalWrite(TrackSensorRightPin2, HIGH);

  //pin mode for HC-SR04
	pinMode(trigPin, OUTPUT);  
	pinMode(echoPin, INPUT);

  ResetData();
  
  // Comfigure NRF24L01
  // initialize the transceiver on the SPI bus
  if (!radio.begin()) {
    Serial.println(F("radio hardware is not responding!!"));
    //while (1) {}  // hold in infinite loop
  }
  // Set the PA Level low to try preventing power supply related problems
  // because these examples are likely run with nodes in close proximity to
  // each other.
  //radio.setPALevel(RF24_PA_LOW);  // RF24_PA_MAX is default. 

  // save on transmission time by setting the radio to only transmit the
  // number of bytes we need to transmit a float
  radio.setPayloadSize(sizeof(payload));  // float datatype occupies 4 bytes
  
  // set the RX address of the TX node into a RX pipe
  radio.openReadingPipe(1, address[0]);  // using pipe 1
  // Serial.println((int)address[0]);
  // Serial.println(pipeIn); 
  //radio.openReadingPipe(1,pipeIn);//Same as the code from the transmitter,
  radio.startListening(); //start the radio comunication for receiver
  //pinMode(LED_BUILTIN,OUTPUT);//LED推挽输出
  //digitalWrite(LED_BUILTIN,HIGH);
  
} // Setup

//Function of receiving remote data
unsigned long lastRecvTime = 0;
void recvData()
  {
    lastRecvTime = 0;
    while (radio.available() ) 
    {
      radio.read(&data, sizeof(Signal));//receive the data 
      lastRecvTime = millis();   //Current time ms
    }
  }

void loop(){
  //Ultrasonic distence
  DistanceDetecting();

  // RX node message &pipe
   //uint8_t pipe;
   //if (radio.available(&pipe)) {              // is there a payload? get the pipe number that recieved it
        //uint8_t bytes = radio.getPayloadSize();  // get the size of the payload
        //radio.read(&payload, bytes);             // fetch payload from FIFO
        //Serial.print(F("Received "));
        //Serial.print(bytes);  // print the size of the payload
        //Serial.print(F(" bytes on pipe "));
        //Serial.print(pipe);  // print the pipe number
        //Serial.print(F(": "));
        //Serial.println(payload);  // print the payload's value
     //}
     
  //Receive data from Transmitter
  recvData();

  //lastRecvTime = 0;
  //Check signal from remote, Enter to Line follower mode when lost
  unsigned long now = millis();
  if ( now - lastRecvTime > 1000 ) 
  {
    //ResetData(); //Time out large than 1 second, Signal lost.. Reset data
    Serial.print("Remote signal Timeout > 1s, ");
    Serial.println("Enter line follower mode");

    //Start moving
    //data.roll = 50;
    data.pitch = 50;
    //Line following
    LineFollowing();    
   }// end if for checking Signal lost or Timeout
  
  // Write/Output the PWM signal
  //Moving forward or backward
  if (data.pitch < 128) {
      //Moving forward A-1A = 1/PWM, A-1B = 0 
      //data.MovingSpeed_A1A = 129 - data.pitch;
      data.MovingSpeed_A1A = 0;
      data.MovingSpeed_A1B = 150; //(Reverse motor wiring when L9110 defect on AB=10)
    }
    else {
      //Moving backward A-1A = 1/PWM, A-1B = 1/PWM
      //data.MovingSpeed_A1A = 129 - data.pitch;
      data.MovingSpeed_A1A = 0;
      data.MovingSpeed_A1B = 140;
  } //End of if-else for moving

    //Turn leftward or rightward
  if (data.yaw < 127) {
      //Turn leftward B-1A = 1/PWM, B-1B = 0
      data.Turn_B1A = 150;//127 - data.yaw;
      data.Turn_B1B = 0;
    }
    else if (data.yaw > 129) {
      //Turn rightward B-1A = 1/PWM, B-1B = 1/PWM
      data.Turn_B1A = 0;
      data.Turn_B1B = 150;//data.yaw - 10;
    }
    else {
      data.Turn_B1A = 0; // straight
      data.Turn_B1B = 0;      

    }
  //end of if-else for Turn

  // if distance > 20 cm, go ahead, otherwise stop
  if (distance < 2)  {
    data.MovingSpeed_A1A = 0;
    data.MovingSpeed_A1B = 0;
    data.Turn_B1A = 0;
    data.Turn_B1B = 0;
    }// end of if for distance checking

  //writing PWM signal to the drive: speed
  analogWrite(D5, data.MovingSpeed_A1A);
  analogWrite(D7, data.MovingSpeed_A1B);  
  //writing PWM signal to the drive: trun
  analogWrite(D3, data.Turn_B1A);
  analogWrite(D6, data.Turn_B1B);
  Serial.print("\t"); Serial.print(data.MovingSpeed_A1A);
  Serial.print("\t"); Serial.print(data.MovingSpeed_A1B);
  Serial.print("\t"); Serial.print(data.Turn_B1A);
  Serial.print("\t"); Serial.println(data.Turn_B1B);

  delay (1500);
  //Stop trun
  analogWrite(D3, 0);
  analogWrite(D6, 0);
  delay (500);
} // loop

void LineFollowing() {
    //检测到黑线时循迹模块相应的指示灯亮，端口电平为LOW
    //未检测到黑线时循迹模块相应的指示灯灭，端口电平为HIGH
    TrackSensorLeftValue1  = digitalRead(TrackSensorLeftPin1);
    TrackSensorLeftValue2  = digitalRead(TrackSensorLeftPin2);
    TrackSensorRightValue1 = digitalRead(TrackSensorRightPin1);
    TrackSensorRightValue2 = digitalRead(TrackSensorRightPin2);
    Serial.print("\t"); Serial.print(TrackSensorRightValue2);
    Serial.print("\t"); Serial.print(TrackSensorRightValue1);    
    Serial.print("\t"); Serial.print(TrackSensorLeftValue2);
    Serial.print("\t"); Serial.println(TrackSensorLeftValue1);
    //四路循迹引脚电平状态
    // 0 0 X 0
    // 1 0 X 0
    // 0 1 X 0
    //以上6种电平状态时小车原地右转，速度为250,延时80ms
    //处理右锐角和右直角的转动
    if ( (TrackSensorLeftValue1 == LOW || TrackSensorLeftValue2 == LOW) &&  TrackSensorRightValue2 == LOW)
    {
      //spin_right(250, 250);
      data.pitch = 80;
      data.yaw = 200;
      delay(80);
    }
    //四路循迹引脚电平状态
    // 0 X 0 0       
    // 0 X 0 1 
    // 0 X 1 0       
    //处理左锐角和左直角的转动
    else if ( TrackSensorLeftValue1 == LOW && (TrackSensorRightValue1 == LOW ||  TrackSensorRightValue2 == LOW))
    {
      //spin_left(250, 250);
      data.pitch = 80;
      data.yaw = 50;
      delay(80);
    }
    // 0 X X X
    //最左边检测到
    else if ( TrackSensorLeftValue1 == LOW)
    {
      //spin_left(150, 150);
      data.pitch = 150;
      data.yaw = 50;
      //delay(2);
    }
    // X X X 0
    //最右边检测到
    else if ( TrackSensorRightValue2 == LOW )
    {
      //spin_right(150, 150);
      data.pitch = 80;
      data.yaw = 200;
      //delay(2);
    }
    //四路循迹引脚电平状态
    // X 0 1 X
    //处理左小弯
    else if ( TrackSensorLeftValue2 == LOW && TrackSensorRightValue1 == HIGH)
    {
      //left(0, 220);
      data.pitch = 80;
      data.yaw = 50;
    }
    //四路循迹引脚电平状态
    // X 1 0 X  
    //处理右小弯
    else if (TrackSensorLeftValue2 == HIGH && TrackSensorRightValue1 == LOW)
    {
      //right(220, 0);
      data.pitch = 80;
      data.yaw = 200;
    }
    //四路循迹引脚电平状态
    // X 0 0 X
    //处理直线
    else if (TrackSensorLeftValue2 == LOW && TrackSensorRightValue1 == LOW)
    {
      //run(255, 255);
      data.pitch = 20;
      data.yaw = 127;
    }
    //当为1 1 1 1时(No line detected)小车保持上一个小车运行状态
 
} //End of Function for line folloowing

/**
*https://projecthub.arduino.cc/Isaac100/getting-started-with-the-hc-sr04-ultrasonic-sensor-7cabe1
*time x speed = distance, The speed of sound is approximately 340 meters per second
*but since the pulseIn() function returns the time in microseconds, 
*"speed of sound in centimeters per microsecond" will say that it is .0343 cm/μS
*multiply the duration by .0343 and then divide it by 2(Because the sound waves travel to the object AND back).
*distance (cm) = (duration (μS) *.0343)/2;
*/
void DistanceDetecting() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  Time_Echo_us = pulseIn(echoPin, HIGH);
  if ((Time_Echo_us < 60000) && (Time_Echo_us >1))
  {
    Length_mm_X100 = (Time_Echo_us * 34)/2;
    Length_Integer = Length_mm_X100/100;
    Length_Fraction = Length_mm_X100%100;
    distance = Length_mm_X100/1000;
    Serial.print("Distance(cm): ");
    Serial.println(distance);
  }
} //End of Function for distance detecting